Robust Bayesian State Estimation and Mapping

Virtually all robotic and autonomous systems rely on navigation and mapping algorithms (e.g. the Kalman filter or simultaneous localization and mapping (SLAM)) to determine their location in the world. Unfortunately, these algorithms are not robust to outliers and even a single faulty measurement can cause a catastrophic failure of the navigation system. This thesis proposes several novel robust navigation and SLAM algorithms that produce accurate results when outliers and faulty measurements occur. The new algorithms address the robustness problem by augmenting the standard models used by filtering and SLAM algorithms with additional latent variables that can be used to infer when outliers have occurred. Solving the augmented problems leads to algorithms that are naturally robust to outliers and are nearly as efficient as their non-robust counterparts. The first major contribution of this thesis is a novel robust filtering algorithm that can compensate for both measurement outliers and state prediction errors using a set of sparse latent variables that can be inferred using an efficient convex optimization. Next the thesis proposes a batch robust SLAM algorithm that uses the ExpectationMaximization algorithm to infer both the navigation solution and the measurement information matrices. Inferring the information matrices allows the algorithm to reduce the impact of outliers on the SLAM solution while the Expectation-Maximization procedure produces computationally efficient calculations of the information matrix estimates. While several SLAM algorithms have been proposed that are robust to loop closure errors, to date no SLAM algorithms have been developed that are robust to landmark errors. The final contribution of this thesis is the first SLAM algorithm that is robust to both loop closure and landmark errors (incremental SLAM with consistency checking (ISCC)). ISCC adds integer variables to the SLAM optimization that indicate whether each measurement should be included in the SLAM solution. ISCC then uses an incremental greedy strategy to efficiently determine which measurements should be used to compute the SLAM solution. Evaluation on standard benchmark datasets as well as visual SLAM experiments demonstrate that ISCC is robust to a